//
// Created by skywoodsz on 2023/3/13.
//
// copy from:

#ifndef SRC_HOQP_H
#define SRC_HOQP_H

#include "qm_wbc/Task.h"
#include <memory>

namespace qm
{
// Hierarchical Optimization Quadratic Program
class HoQp{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    using HoQpPtr = std::shared_ptr<HoQp>;

    explicit HoQp(const Task& task) : HoQp(task, nullptr){};

    HoQp(Task task, HoQpPtr higherProblem);

    matrix_t getStackedZMatrix() const { return stackedZ_; }

    Task getStackedTasks() const { return stackedTasks_; }

    vector_t getStackedSlackSolutions() const { return stackedSlackVars_; }

    vector_t getSolutions() const {
        vector_t x = xPrev_ + stackedZPrev_ * decisionVarsSolutions_;
        return x;
    }

    size_t getSlackedNumVars() const { return stackedTasks_.d_.rows(); }

private:
    void initVars();
    void formulateProblem();
    void solveProblem();

    void buildHMatrix();
    void buildCVector();
    void buildDMatrix();
    void buildFVector();

    void buildZMatrix();
    void stackSlackSolutions();

    Task task_, stackedTasksPrev_, stackedTasks_;
    HoQpPtr higherProblem_;

    bool hasEqConstraints_{}, hasIneqConstraints_{};
    size_t numSlackVars_{}, numDecisionVars_{};
    matrix_t stackedZPrev_, stackedZ_;
    vector_t stackedSlackSolutionsPrev_, xPrev_;
    size_t numPrevSlackVars_{};

    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> h_, d_;
    vector_t c_, f_;
    vector_t stackedSlackVars_, slackVarsSolutions_, decisionVarsSolutions_;

    // Convenience matrices that are used multiple times
    matrix_t eyeNvNv_;
    matrix_t zeroNvNx_;
};
}


#endif //SRC_HOQP_H
